/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include <GL/gl.h>
#include <GL/glu.h>
#include <GL/glut.h>
#include <string.h>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <sys/wait.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <time.h>
#include <unistd.h>
#include <signal.h>
#include "voslam_image.h"
#include "voslam_blas.h"
#include "voslam_geo.h"
#include "voslam_util.h"
#include "voslam_calc.h"
#include "voslam_bookkeep.h"
#include "voslamdef.h"
#include "jpeg_read.h"

#define BASETYPE3 short

static int width;
static int height;
unsigned char *	img=NULL;
unsigned char * image=NULL;
unsigned char *	imC=NULL;
int Window_ID,Window_ID2;


static int numpts=0,*maski,max_num_of_features=300;
static double *ptsA,*ptsB,*XX,*fu,*fv,*iu,*iv;
int sockfd,num_landmarks; 
struct timespec start, stop;
static double runtime,param[6],X0[10];

int window=100;
int hit=1;
int iter=7;
int radial_distortion=1;
int image_resize=0;
int press=0;
int num=0;
int end=1;
int step=1;
char *runko="kuva";
int *list;
FILE* list_fid;
double vz[12];
float tnorm=1.;
int num_tpts,num_tracked_points;
double tpts[22];
double tX[38];
int   process_image(unsigned char* image);
/* ./gl_voslam_mono -m 500  -n ~/jsvn/GIM/staticdata/datasets/Marsubot_Tahvo/laboratory_corridor_left_hand_side_of_2554_coffee_room/Tahvo.MaCI_Image.ExampleCam_05_12_2010_12_38_53/  -i 9  -e 500 -w 5 -p p4.dat -r -l listtahvo1.txt -z 0.05  >log1
./gl_voslam_mono -m 500  -n ~/jsvn/GIM/staticdata/datasets/J2B2/08_02_11/KinectDataSet/KinectDevice.MaCI_Image.CameraData_08_02_2011_17:56:51/  -i 1  -e 500 -w 5 -p pk.dat -r -l listkinect1.txt -z 0.05  >log1
./gl_voslam_mono -m 500  -n ~/jsvn/GIM/staticdata/datasets/Avant635/08_12_2010/Avant635.MaCI_Image.Firewire_08_12_2010_11:25:13/  -i 9  -e 500 -w 5 -p pu1.dat  -l listavant61.txt -z 0.05  >log1
ls ~/jsvn/GIM/src/staticdata/datasets/Avant635/19_04_2011/thirdDataset/Stereobox.MaCI_Image.Firewire\ JPEG_19_04_2011_15\:36\:55/1/ > listav.txt  
./gl_voslam_mono -m 500  -n ~/jsvn/GIM/src/staticdata/datasets/Avant635/19_04_2011/thirdDataset/Stereobox.MaCI_Image.Firewire\ JPEG_19_04_2011_15\:36\:55/1/  -i 9  -e 500 -w 5 -p pr.dat  -l listav.txt -z 0.05  >log1 
./gl_voslam_mono -m 500  -n ~/jsvn/GIM/src/staticdata/datasets/Avant635/19_04_2011/thirdDataset/Stereobox.MaCI_Image.Firewire\ JPEG_19_04_2011_15\:36\:55/  -i 1  -e 117 -w 6 -p pn1.dat  -l listav1.dat -z 0.05  >log2
./gl_voslam_mono -m 500  -n ~/jsvn/GIM/src/staticdata/datasets/Avant635/19_04_2011/secondDataset/Stereobox.MaCI_Image.Firewire\ JPEG_19_04_2011_15\:27\:41/1/  -i 1  -e 430 -w 7 -p pr.dat  -l listav2.dat -z 0.05  >log1 

*/

/**********************************************************************
 *
 * Color correction functions
 *
 **********************************************************************/

/*
 * Turn a YUV4:2:0 block into an RGB block
 *
 * Video4Linux seems to use the blue, green, red channel
 * order convention-- rgb[0] is blue, rgb[1] is green, rgb[2] is red.
 *
 * Color space conversion coefficients taken from the excellent
 * http://www.inforamp.net/~poynton/ColorFAQ.html
 * In his terminology, this is a CCIR 601.1 YCbCr -> RGB.
 * Y values are given for all 4 pixels, but the U (Pb)
 * and V (Pr) are assumed constant over the 2x2 block.
 *
 * To avoid floating point arithmetic, the color conversion
 * coefficients are scaled into 16.16 fixed-point integers.
 * They were determined as follows:
 *
 *	double brightness = 1.0;  (0->black; 1->full scale) 
 *	double saturation = 1.0;  (0->greyscale; 1->full color)
 *	double fixScale = brightness * 256 * 256;
 *	int rvScale = (int)(1.402 * saturation * fixScale);
 *	int guScale = (int)(-0.344136 * saturation * fixScale);
 *	int gvScale = (int)(-0.714136 * saturation * fixScale);
 *	int buScale = (int)(1.772 * saturation * fixScale);
 *	int yScale = (int)(fixScale);	
 */

/* LIMIT: convert a 16.16 fixed-point value to a byte, with clipping. */
#define LIMIT(x) ((x)>0xffffff?0xff: ((x)<=0xffff?0:((x)>>16)))

static inline void
move_420_block(int yTL, int yTR, int yBL, int yBR, int u, int v, 
	       int rowPixels, unsigned char * rgb, int bits)
{
	const int rvScale = 91881;
	const int guScale = -22553;
	const int gvScale = -46801;
	const int buScale = 116129;
	const int yScale  = 65536;
	int r, g, b;

	g = guScale * u + gvScale * v;
//	if (force_rgb) {
//		r = buScale * u;
//		b = rvScale * v;
//	} else {
		r = rvScale * v;
		b = buScale * u;
//	}

	yTL *= yScale; yTR *= yScale;
	yBL *= yScale; yBR *= yScale;

	if (bits == 24) {
		/* Write out top two pixels */
		rgb[0] = LIMIT(b+yTL); rgb[1] = LIMIT(g+yTL);
		rgb[2] = LIMIT(r+yTL);

		rgb[3] = LIMIT(b+yTR); rgb[4] = LIMIT(g+yTR);
		rgb[5] = LIMIT(r+yTR);

		/* Skip down to next line to write out bottom two pixels */
		rgb += 3 * rowPixels;
		rgb[0] = LIMIT(b+yBL); rgb[1] = LIMIT(g+yBL);
		rgb[2] = LIMIT(r+yBL);

		rgb[3] = LIMIT(b+yBR); rgb[4] = LIMIT(g+yBR);
		rgb[5] = LIMIT(r+yBR);
	} else if (bits == 16) {
		/* Write out top two pixels */
		rgb[0] = ((LIMIT(b+yTL) >> 3) & 0x1F) 
			| ((LIMIT(g+yTL) << 3) & 0xE0);
		rgb[1] = ((LIMIT(g+yTL) >> 5) & 0x07)
			| (LIMIT(r+yTL) & 0xF8);

		rgb[2] = ((LIMIT(b+yTR) >> 3) & 0x1F) 
			| ((LIMIT(g+yTR) << 3) & 0xE0);
		rgb[3] = ((LIMIT(g+yTR) >> 5) & 0x07) 
			| (LIMIT(r+yTR) & 0xF8);

		/* Skip down to next line to write out bottom two pixels */
		rgb += 2 * rowPixels;

		rgb[0] = ((LIMIT(b+yBL) >> 3) & 0x1F)
			| ((LIMIT(g+yBL) << 3) & 0xE0);
		rgb[1] = ((LIMIT(g+yBL) >> 5) & 0x07)
			| (LIMIT(r+yBL) & 0xF8);

		rgb[2] = ((LIMIT(b+yBR) >> 3) & 0x1F)
			| ((LIMIT(g+yBR) << 3) & 0xE0);
		rgb[3] = ((LIMIT(g+yBR) >> 5) & 0x07)
			| (LIMIT(r+yBR) & 0xF8);
	}
}

/* Converts from planar YUV420 to RGB24. */
void 
yuv420p_to_rgb(int width, int height,
	       unsigned char *pIn0, unsigned char *pOut0, int bits)
{
	const int numpix = width * height;
	const int bytes = bits >> 3;
	int i, j, y00, y01, y10, y11, u, v;
	unsigned char *pY = pIn0;
	unsigned char *pU = pY + numpix;
	unsigned char *pV = pU + numpix / 4;
	unsigned char *pOut = pOut0;

	for (j = 0; j <= height - 2; j += 2) {
		for (i = 0; i <= width - 2; i += 2) {
			y00 = *pY;
			y01 = *(pY + 1);
			y10 = *(pY + width);
			y11 = *(pY + width + 1);
			u = (*pU++) - 128;
			v = (*pV++) - 128;

			move_420_block(y00, y01, y10, y11, u, v,
				       width, pOut, bits);
	
			pY += 2;
			pOut += 2 * bytes;

		}
		pY += width;
		pOut += width * bytes;
	}
}
/*macro used to convert a YUV pixel to RGB format
  from Bart Nabbe
YUV2BGR*/
#define YUV2RGB(y, u, v, r, g, b)\
  b = y + ((v * 1434) / 2048);\
  g = y - ((u * 406) / 2048) - ((v * 595) / 2048);\
  r = y + ((u * 2078) / 2048);\
  r = r < 0 ? 0 : r;\
  g = g < 0 ? 0 : g;\
  b = b < 0 ? 0 : b;\
  r = r > 255 ? 255 : r;\
  g = g > 255 ? 255 : g;\
  b = b > 255 ? 255 : b


int yuv422torgb (char *YUV, char *RGB, int NumPixels) {
  int i, j;
  register int y0, y1, u, v;
  register int r, g, b;

  for (i = 0, j = 0; i < 2 * NumPixels; i += 4, j += 6)
    {
      u = (unsigned char) YUV[i + 0] - 128;
      y0 = (unsigned char) YUV[i + 1];
      v = (unsigned char) YUV[i + 2] - 128;
      y1 = (unsigned char) YUV[i + 3];
      YUV2RGB (y0, u, v, r, g, b);
      RGB[j + 0] = r;
      RGB[j + 1] = g;
      RGB[j + 2] = b;
      YUV2RGB (y1, u, v, r, g, b);
      RGB[j + 3] = r;
      RGB[j + 4] = g;
      RGB[j + 5] = b;
    }
  return 0;
}

void ppm_save(unsigned char* im,char* filename,int width,int height)
{
  FILE* fd;
 

  fd=fopen(filename,"wb");
  if(fd==NULL)
    {
      printf("error\n");
      exit(-1);
    }
  fprintf(fd,"P6\n");
  fprintf(fd,"%d %d\n",width,height);
  fprintf(fd,"255\n");

  fwrite(im,1,3*width*height,fd);

  fclose(fd);
}

void pgm_save(unsigned char* im,char* filename,int width,int height)
{
  FILE* fd;
  int i,j;

  fd=fopen(filename,"wb");
  if(fd==NULL)
    {
      printf("error\n");
      exit(-1);
    } 

  fprintf(fd,"P2\n");
  fprintf(fd,"%d %d\n",width,height);
  fprintf(fd,"255\n");

  for(i=0;i<height;i++)
    {
      for(j=0;j<width;j++)
	{
	  fprintf(fd,"%d ",(int)im[i*(width)+j]);
	}
      fprintf(fd,"\n");
    }
  fclose(fd);
}

void initGL(void)
{

	glPixelStorei ( GL_UNPACK_ALIGNMENT, 1 );
	glClearColor ( 0.0F, 0.0F, 0.0F, 0.0F );
	glViewport ( 0, 0, width, height );

	/* set coordinate frame for graphics in window */
	glMatrixMode ( GL_PROJECTION );
	glLoadIdentity();

	gluOrtho2D ( 0, width, height, 0 );

	glMatrixMode ( GL_MODELVIEW );
	glLoadIdentity();

	/* set zoom factors for image display */
	glPixelZoom ( 1, -1 );

   /* set origin for drawing images as the top-left corner of the window */
	glRasterPos2i ( 0, 0 );
}

void renderScene(void) {
int i;
double x[3];
	glutSetWindow(Window_ID);
	glClear(GL_COLOR_BUFFER_BIT);
	glDrawPixels ( width, height,GL_LUMINANCE,GL_UNSIGNED_BYTE,image );
//	printf("num_pts:%d\n",numpts);
	glColor3f(0.,0.,255.);
	glPointSize(5.);
	glBegin(GL_POINTS);
	for(i=0;i<numpts;i++)
	{	
		if(maski[i]<0)
		    glColor3f(0.,0.,255.);
		else if(maski[i]==0)
		    glColor3f(0.,255.,0.);
		else
		    glColor3f(255.,0.,0.);
		glVertex2f((GLfloat)(ptsB[2*i+1]+2.),(GLfloat)(ptsB[2*i]+2.));
        }
	glEnd();
	
	
	glColor3f(255.,0.,255.);
	glPointSize(15.);
	glBegin(GL_POINTS);
	for(i=0;i<num_tracked_points;i++)
	{	
	   
	    proj3D(vz,param,x,tX+3*i,1);
	    if(x[2]>0)
	    {
	        glVertex2f((GLfloat)x[1],(GLfloat)x[0]);
	       
	    }
	}
	glEnd();
	
	glBegin(GL_LINES);
	for(i=0;i<numpts;i++)
	{
	        if(maski[i]<0)
		    glColor3f(0.,0.,255.);
		else if(maski[i]==0)
		    glColor3f(0.,255.,0.);
		else
		    glColor3f(255.,0.,0.);
		glVertex2f((GLfloat)(ptsB[2*i+1]+2.),(GLfloat)(ptsB[2*i]+2.));
		glVertex2f((GLfloat)(ptsA[2*i+1]+2.),(GLfloat)(ptsA[2*i]+2.));
	}
	glEnd();
	glFlush();
	glutSwapBuffers();
}
void captureScene(void) {

    int len,retp;
    char filename[1024],*ret,file[128];

    if(hit)
    {
      
        if(num<end)
        {

            clock_gettime(CLOCK_REALTIME,&start);
            ret=fgets(file,128,list_fid);
            if(ret==NULL)
                exit(0);
        
            len=strlen(file);
            file[len-1]='\0';
            sprintf(filename,"%s%s",runko,file);
            printf("%s\n",filename);

           
            if(radial_distortion||image_resize)
            {
                jpeg_read(img,filename,&width,&height);
                
            }
            else
            {
                jpeg_read(image,filename,&width,&height);
            }
    
            runtime = -(stop.tv_sec - start.tv_sec) - (double)(stop.tv_nsec - start.tv_nsec) / 1000000000.0;

         
           stop=start;
           if(radial_distortion)
           {
              warp_image_fast(image,img,fu,fv,width,height);
           }
           if(image_resize)
           {
              width=width/2;
              height=height/2;
              pyr_down(img,image,width,height);
           }
           retp=process_image(image);
           if(retp==0)
           {
             num+=step;
             return;
           }
           printf("filename: %d %s\n",num,filename);
           num+=step;
           glutSetWindow(Window_ID);
           glutPostRedisplay();    

        }
        if(press)
            hit=0;
        usleep(100000);
    }
    else
    {
        usleep(10000);
    }
}

    
void changeSize(int w, int h) {

	glutSetWindow(Window_ID);	/* Set the viewport to be the entire window */
	glViewport(0, 0, w, h);
	
	/* Reset the coordinate system before modifying */
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluOrtho2D ( 0, width, height, 0 );
	
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
	/* set zoom factors for image display */
	glPixelZoom ( (float)w/(float)width, -(float)h/(float)height );
	
	/* set origin for drawing images as the top-left corner of the window */
	glRasterPos2i ( 0, 0 );

}

void Key(
   unsigned char key, 
   int x, int y
)
{
   int i;
   switch (key) {
      case 113: case 81: case 27: /* Q (Escape) - We're outta here. */
       for(i=0;i<num_landmarks;i++)
	{
	    printf("XX: %f %f %f \n",XX[3*i],XX[3*i+1] ,XX[3*i+2]);
	}
      	glutDestroyWindow(Window_ID);
      	exit(1);
      break;/* exit doesn't return, but anyway... */
      case 's':
		ppm_save(img,"kuva.ppm",width,height);
      break; 
      case ' ':
        hit=0;
        press=1;
      break;
      case 'n':
      		num++;
		hit=1;
      break;
      case 'c':
        hit=1;
        press=0;
      break;
      }
}
void processMouse(int button, int state, int x, int y)
{
    if (state == GLUT_DOWN)
    {
         if (button == GLUT_LEFT_BUTTON) 
         {
             tpts[2*num_tpts]=y;
             tpts[2*num_tpts+1]=x;
          if(num_tpts<20)
              num_tpts++;
         } 
         glutSetWindow(Window_ID);
         glutPostRedisplay();    
    }
} 

  
int main(int argc, char* argv[])
{
        double pts[3],ptsw[3];
 	int arg,i,j,ret,len;
	FILE* fd;
	char *x_file="x0.dat",*buf;
	char *calib="p.dat";
	char *listfile="list.txt",filename[1024],file[128];


   clock_gettime(CLOCK_REALTIME,&start);
   srand(start.tv_sec);

        list=(int *)malloc(65536*sizeof(int));
	
	for(arg=1; arg < argc; ++arg)
	{
		char *p=argv[arg];
		if (*p == 0)
			continue;
		if (*p == '-')
		{
			while (*++p)
			{
				switch(*p)
				{
				      case 'h':
		        fprintf(stderr,"\t\n");
                        fprintf(stderr," Usage: ./gl_voslam_mono [options] \n");
			fprintf(stderr,"[-i num]\t index of the first image to read\n");
			fprintf(stderr,"[-e num]\t index of the last image to read\n");
			fprintf(stderr,"[-s num]\t read every s image\n");
                        fprintf(stderr,"[-m num]\t maximum number of landmarks extracted from single image\n");
			fprintf(stderr,"[-n string]\t path to the images including the base image name \n");
			fprintf(stderr,"[-c  num]\t number of optimisation iterations in slam calculations\n");
			fprintf(stderr,"[-w  num]\t  number of consecutive poses estimated in slam calculations\n");
			fprintf(stderr,"[-p filename]\t filename of file containing 6 calibration parameters for the camera \n");
			fprintf(stderr,"[-x filename]\t filename of file containing 7 parameters for initial pose\n");
			fprintf(stderr,"[-l filename]\t filename of file containing indeces of images\n");
			fprintf(stderr,"[-R ]\t the halve the resolution of the images\n");
			fprintf(stderr,"[-r ]\t if there is no undistortion in the images\n");
			fprintf(stderr,"\t\n");
			return 0;
			break;
					case 'i':
						num = atoi(*(p+1) ? p+1 : argv[++arg]);
						end=num+1;
						break;
					case 'e':
						end = atoi(*(p+1) ? p+1 : argv[++arg]);
						break;
					case 's':
						step = atoi(*(p+1) ? p+1 : argv[++arg]);
						break;
					case 'b':
						hit=0;
						break;
                    case 'c':
                        iter = atoi(*(p+1) ? p+1 : argv[++arg]);
                        break;
                    case 'r':
                        radial_distortion=0;
                        break;
                    case 'R':
                        image_resize=1;
                        break;
		     case 'w':
						window = atoi(*(p+1) ? p+1 : argv[++arg]);
						break;
					case 'n':
						runko = *(p+1) ? p+1 : 
								argv[++arg];
						break;
					case 'p':
						calib = *(p+1) ? p+1 : 
								argv[++arg];
						break;
					case 'm':
						max_num_of_features = atoi(*(p+1) ? p+1 : argv[++arg]);
						break;
					case 'x':
						x_file = *(p+1) ? p+1 : 
								argv[++arg];
						break;
					case 'l':
						listfile = *(p+1) ? p+1 : 
								argv[++arg];
						break;
				}
			}
		}
	}
	
	
    list_fid=fopen(listfile,"r");
 	if((int)list_fid<=0)
  	{
	    printf("error: could not open file %s\n",listfile);
	    exit(1);
  	}
  	for(i=0;i<num;i++)
    {
        buf=fgets(file,128,list_fid);
        if(buf==NULL)
        {
            printf("error: could not open file %s\n",filename);
            exit(1);
        }
    }
    buf=fgets(file,128,list_fid);
	if(buf==NULL)
  	{
	    printf("error: could not open file %s\n",filename);
	    exit(1);
  	}
  	len=strlen(file);
    file[len-1]='\0';
  	sprintf(filename,"%s%s",runko,file);
        jpeg_read_i(filename,&width,&height);
	img=(unsigned char *)malloc(width*height*3*sizeof(unsigned char));
	image=(unsigned char *)malloc(width*height*3*sizeof(unsigned char));
        
	if (( ptsA=(double *)malloc(2*max_num_of_features*sizeof(double))) == NULL) printf("malloc error");
	if (( ptsB=(double *)malloc(2*max_num_of_features*sizeof(double))) == NULL) printf("malloc error");
	if (( maski=(int *)malloc(2*max_num_of_features*sizeof(int))) == NULL) printf("malloc error");
	if (( fu=(double *)malloc(width*height*sizeof(double))) == NULL) printf("malloc error");              
	if (( fv=(double *)malloc(width*height*sizeof(double))) == NULL) printf("malloc error");              
	if (( iu=(double *)malloc(width*height*sizeof(double))) == NULL) printf("malloc error");              
	if (( iv=(double *)malloc(width*height*sizeof(double))) == NULL) printf("malloc error"); 

	fd=fopen(calib,"r");
	for(i=0;i<6;i++)
	{
        ret=fscanf(fd,"%lf ",param+i);
        if(ret<=0)
        {
            printf("calib read error\n");
            exit(0);
        }
    }
	fclose(fd);

    pts[0]=param[2];
    param[2]=param[1];
    param[1]=pts[0];
    
    if(radial_distortion)
    {
        for(i=0;i<height;i++)
        {
            for(j=0;j<width;j++)
            {
                pts[0]=i;
	            pts[1]=j;
	            undist_points3(ptsw,pts,param,10,1);
                iu[i*width+j]=ptsw[0];
	            iv[i*width+j]=ptsw[1];
            }
        }
        for(i=0;i<height;i++)
        {
            for(j=0;j<width;j++)
            {
                pts[0]=i;
                pts[1]=j;
                dist_points3(ptsw,pts,param,1);
                fu[i*width+j]=ptsw[0];
                fv[i*width+j]=ptsw[1];
            }
        }
    }
    if(image_resize)
    {
      param[0]=param[0]/2.;
      param[1]=param[1]/2.;
      param[2]=param[2]/2.;
    }
	fd=fopen(x_file,"r");
	for(i=0;i<7;i++)
    {
        ret=fscanf(fd,"%lf ",X0+i);
        if(ret<=0)
        {
            printf("X0 read error\n");
            exit(0);
        }
    }
    fclose(fd);
    clock_gettime(CLOCK_REALTIME,&stop);	

    
    for(i=0;i<5;i++)
        printf("%lf ",param[i]);
    printf("\n ");
    for(i=0;i<7;i++)
        printf("%lf ",X0[i]);
    printf("\n ");    

    glutInit(&argc, argv);
    glutInitDisplayMode(GLUT_RGB | GLUT_DOUBLE);
    glutInitWindowPosition(100, 100);
    glutInitWindowSize(width, height);
    Window_ID=glutCreateWindow("Window 1");

    initGL();
	
    glutDisplayFunc(renderScene);
    glutReshapeFunc(changeSize);
    glutIdleFunc(captureScene);	
    glutKeyboardFunc(&Key);
    glutMouseFunc(processMouse);

    glutMainLoop();
    fclose(list_fid);
    return 0;
}


void init_next_pose(double *v,double *Xpos, double *p, double *res, int N, double th, double *pA, double *XA, double *fA, double *fB,int num_points)
{
    double dev,lambda,dev1,dev0,R[9],v0[10],va[10],norm;
    
    Rt_2_t(v,Xpos);

    v[7]=p[0];
    v[8]=p[1];
    v[9]=p[2];
    va[7]=p[0];
    va[8]=p[1];
    va[9]=p[2];

    quater_2_rot(R,v);
    p3p2dev(&dev1,res,R,v+4,p,pA,XA,N);


    ransac_calc_rot_p(v0,res,&dev,fB,fA,p,num_points);


    v0[4]=0;
    v0[5]=0;
    v0[6]=0;

    motion3D_update(v,v0,va);
    quater_2_rot(R,va);
    p3p2dev(&dev,res,R,va+4,p,pA,XA,N);
    printf("devRR: %d %f \n",num,dev);
    lambda=0.1;
    pose_calct1(va,pA,XA,&lambda,10.,N);
    quater_2_rot(R,va);
    p3p2dev(&dev,res,R,va+4,p,pA,XA,N);
    printf("oudev: %d %f \n",num,dev);
    lambda=0.01;
    
    if(N>3)
    {
        ransac_calc_p3p_v(v0,v0+4,v,v+4,0.5,100,p,res,&dev,pA,XA,N,th);
        printf("q0: %d %f %f %f %f \n",num,v[0],v[1],v[2],v[3]);
        printf("t0: %d %f %f %f  \n",num,v[4],v[5],v[6]);
        norm=sqrt(v0[0]*v0[0]+v0[1]*v0[1]+v0[2]*v0[2]+v0[3]*v0[3]);
        printf("q0norm: %d %f\n",num,norm);
        printf("dev0: %d %f\n",num,dev);
        quater_2_rot(R,v0);
        p3p2dev(&dev0,res,R,v0+4,p,pA,XA,N);
        printf("devi: %d %f\n",num,dev0);
        if(dev0>2.)
        {
            ransac_calc_p3p_v(v0,v0+4,v,v+4,0.5,100,p,res,&dev,pA,XA,N,th);
            quater_2_rot(R,v0);
            p3p2dev(&dev0,res,R,v0+4,p,pA,XA,N);
            printf("devi2: %d %f\n",num,dev0);
        }

        dev=dev0;
        v[0]=v0[0];
        v[1]=v0[1];
        v[2]=v0[2];
        v[3]=v0[3];
        v[4]=v0[4];
        v[5]=v0[5];
        v[6]=v0[6];
        if(dev1<dev)
        {
            v[0]=Xpos[0];
            v[1]=Xpos[1];
            v[2]=Xpos[2];
            v[3]=Xpos[3];
            v[4]=Xpos[4];
            v[5]=Xpos[5];
            v[6]=Xpos[6];
            dev=dev1;
        }
        printf("dev2: %f \n",dev);
        lambda=10;//0.001
        pose_calct1(v,pA,XA,&lambda,2.*dev,N);
        quater_2_rot(R,v);
        p3p2dev(&dev,res,R,v+4,p,pA,XA,N);
        printf("dev: %f \n",dev);
        lambda=0.1;
        pose_calct1(v,pA,XA,&lambda,2.*dev,N);
        quater_2_rot(R,v);
        p3p2dev(&dev,res,R,v+4,p,pA,XA,N);
        printf("dev: %f \n",dev);
                
        lambda=0.01;
        pose_calct1(v,pA,XA,&lambda,2.*dev,N);
        quater_2_rot(R,v);
        p3p2dev(&dev,res,R,v+4,p,pA,XA,N);
        printf("dev: %f \n",dev);
        
        lambda=0.00001;
        pose_calct1(v,pA,XA,&lambda,2.*dev,N);
        quater_2_rot(R,v);
        p3p2dev(&dev,res,R,v+4,p,pA,XA,N);
        printf("dev: %f \n",dev);
        lambda=0.00001;
        pose_calct1(v,pA,XA,&lambda,2.*dev,N);
        quater_2_rot(R,v);
        p3p2dev(&dev,res,R,v+4,p,pA,XA,N);
        printf("vdev: %d %f \n",num,dev);
        if(dev>15)
        {
        lambda=1;//0.001
        pose_calct1(v,pA,XA,&lambda,1000,N);
        quater_2_rot(R,v);
        p3p2dev(&dev,res,R,v+4,p,pA,XA,N);
        printf("idev: %f \n",dev);
        lambda=0.1;
        pose_calct1(v,pA,XA,&lambda,1.5*dev,N);
        quater_2_rot(R,v);
        p3p2dev(&dev,res,R,v+4,p,pA,XA,N);
        printf("dev: %f \n",dev);
                
        lambda=0.01;
        pose_calct1(v,pA,XA,&lambda,1.5*dev,N);
        quater_2_rot(R,v);
        p3p2dev(&dev,res,R,v+4,p,pA,XA,N);
        printf("dev: %f \n",dev);
        
        lambda=0.00001;
        pose_calct1(v,pA,XA,&lambda,dev,N);
        quater_2_rot(R,v);
        p3p2dev(&dev,res,R,v+4,p,pA,XA,N);
        printf("dev: %f \n",dev);
        lambda=0.00001;
        pose_calct1(v,pA,XA,&lambda,dev,N);
        quater_2_rot(R,v);
        p3p2dev(&dev,res,R,v+4,p,pA,XA,N);
   
        }
        printf("fidev: %d %f \n",num,dev);
    }
    if(dev0<dev)
    {
        v[0]=v0[0];
        v[1]=v0[1];
        v[2]=v0[2];
        v[3]=v0[3];
        v[4]=v0[4];
        v[5]=v0[5];
        v[6]=v0[6];
    }

        /* need to check residual after rotation*/
    t_2_Rt(Xpos+7,v);


            
}
void init_structure(double *v, double *X,double *count,double *Xpos,double *pts1, double *pts2, int *indiX, int  *ind2, int *maski, int N, double thx)
{
    int k;

    robust_triangulate1(X,Xpos,v,pts1,pts2,ind2,indiX,maski,thx,&k,N);
    count[3]=k;

}
void find_2closest(int *ind,double *point,double *points,int num_points)
{
    double x,y,xi,yi,dx,dy,dist,mindist,mindist2; 
    int i;
 
    x=point[0];
    y=point[1];

    xi=points[0];
    yi=points[1];
    dx=x-xi;
    dy=y-yi;
    mindist=dx*dx+dy*dy;
    ind[0]=0;
    xi=points[2];
    yi=points[3];
    dx=x-xi;
    dy=y-yi;
    dist=dx*dx+dy*dy;
    if(dist<mindist)
    {
        mindist2=mindist;
        mindist=dist;
        ind[1]=ind[0];
        ind[0]=1;
    }
    else
    {

        mindist2=dist;
        ind[1]=1;
    }

    for(i=2;i<num_points;i++)
    {
        
        xi=points[2*i];
        yi=points[2*i+1];
        dx=x-xi;
        dy=y-yi;
        dist=dx*dx+dy*dy;
        if(dist<mindist)
        {
            ind[1]=ind[0];
            ind[0]=i;
            mindist2=mindist;
            mindist=dist;
        }
        else if(dist<mindist2)
        {
            mindist2=dist;
            ind[1]=i;
        }
    }
}

int is_inside_triangle(double  *vert0,double  *vert1,double  *vert2,double *point)
{
    double n1[3],n2[3],n3[3],di1,di;
    
    /* vectors*/
    
    n1[0]=vert0[1]-vert1[1];
    n1[1]=-vert0[0]+vert1[0];
    n1[2]=-n1[0]*vert0[0]-n1[1]*vert0[1];
    n2[0]=vert0[1]-vert2[1];
    n2[1]=-vert0[0]+vert2[0];
    n2[2]=-n2[0]*vert0[0]-n2[1]*vert0[1];
    n3[0]=vert2[1]-vert1[1];
    n3[1]=-vert2[0]+vert1[0];
    n3[2]=-n3[0]*vert2[0]-n3[1]*vert2[1];
    

    di=n1[0]*vert2[0]+n1[1]*vert2[1]+n1[2];
    di1=n1[0]*point[0]+n1[1]*point[1]+n1[2];
    if(di1>0)
    {
      if(di<0)
      {
        return 0;
      }
    }
    else
    {
      if(di>0)
      {
        return 0;
      }
    }
    di=n2[0]*vert1[0]+n2[1]*vert1[1]+n2[2];
    di1=n2[0]*point[0]+n2[1]*point[1]+n2[2];
    if(di1>0)
    {
      if(di<0)
      {
        return 0;
      }
    }
    else
    {
      if(di>0)
      {
        return 0;
      }
    }
    di=n3[0]*vert0[0]+n3[1]*vert0[1]+n3[2];
    di1=n3[0]*point[0]+n3[1]*point[1]+n3[2];
    if(di1>0)
    {
      if(di<0)
      {
        return 0;
      }
    }
    else
    {
      if(di>0)
      {
        return 0;
      }
    }
    return 1;
    
    
}

int find_triangle(int *ind, double * point,double *points,int num_points)
{
    double dist,mindist;
    int i;
    
    find_2closest(ind,point,points,num_points);
    ind[2]=-1;
    mindist=-1;
    for(i=0;i<num_points;i++)
    {
        if(i!=ind[0]&&i!=ind[1])
        {
            if(is_inside_triangle(points+2*ind[0],points+2*ind[1],points+2*i,point))
            {
                dist=(point[0]-points[2*i])*(point[0]-points[2*i])+(point[1]-points[2*i+1])*(point[1]-points[2*i+1]);
                if(mindist>0)
                {
                    if(dist<mindist)
                    {
                        ind[2]=i;
                        mindist=dist;
                    }
                }
                else
                {
                    ind[2]=i;
                    mindist=dist;
                }
            }
        }
    }
    if(mindist>0)
      return 1;
    else
      return 0; 
}

int estimate_depth_from_neighbours(double *tpts, double *pts, double *vz, double *pA, double *XA,  double *param, int k)
{
    double X1[3],X2[3],X3[3],v1[3],v2[3],n[3],d,tmp[3],scale,iv[7],norm; 
    int ind[3];
    
    find_triangle(ind,tpts,pA,k);
    move3D(vz,XA+ind[0]*3,X1,1);
    move3D(vz,XA+ind[1]*3,X2,1);
     
    if(ind[2]>-1)
    {
        move3D(vz,XA+ind[2]*3,X3,1);
	v1[0]=X2[0]-X1[0];
	v1[1]=X2[1]-X1[1];
	v1[2]=X2[2]-X1[2];
	v2[0]=X3[0]-X1[0];
	v2[1]=X3[1]-X1[1];
	v2[2]=X3[2]-X1[2];
	n[0]=v1[1]*v2[2]-v1[2]*v2[1];
	n[1]=v1[2]*v2[0]-v1[0]*v2[2];
	n[2]=v1[0]*v2[1]-v1[1]*v2[0];
	norm=sqrt(n[0]*n[0]+n[1]*n[1]+n[2]*n[2]);
	n[0]=n[0]/norm;
	n[1]=n[1]/norm;
	n[2]=n[2]/norm;
	d=n[0]*X1[0]+n[1]*X1[1]+n[2]*X1[2];
	normalise_points(pts,tpts,param,1);
	pts[2]=1;
	scale=d/(n[0]*pts[0]+n[1]*pts[1]+n[2]*pts[2]);
	tmp[0]=scale*pts[0];
	tmp[1]=scale*pts[1];
	tmp[2]=scale*pts[2];
	inv_rigid_motion(iv,vz);
	move3D(iv,tmp,pts,1);
	fprintf(stderr,"trpts: %f %f %f %f %f %f %f\n",pts[0],pts[1],pts[2],n[0],n[1],n[2],d);
	return 1;
	
    }
    pts[0]=XA[ind[0]*3];
    pts[1]=XA[ind[1]*3];
    pts[2]=XA[ind[2]*3];
    return 0;
  
}

int process_image(unsigned char *image1)
{


    static double  *fea,*featuresA,*featuresB,*fs,*depth;
    static double *X,*resi,*z,*ptsX,vn[9],*reso,*resl,sume;
    static double th,thx,*Xpos,count[4],*pA,*XA,v[10],C[200],W[10],*fA,*fB,*ptA,*ptB;
    static double *xx,*vx,*vv,*icb,*R,*Ce,*L,*x,*g,*nX,lambda,*Xout,*e,dev[12];
    static int i=0,j,kk,k,*mask,*indx,*indC,*sindex,*tmp,*matches,*swap,cnt,index,*indX,lost=0;
    static int wx,wy,wxy,M,N,num_pts,num_featuresB,num_featuresA,*start,*ind,*indf,*indz,*indA,*indB;
    static int maxV=700,maxX=50000,maxD=15070,maxI=20000,D,num_frames,frame,fea_dim=10,maxIt=10,scyc;
    static int initialized=0,cyc,rwindow,*maskX,*matcho,*matchl,statedim=7,cntr,frame0;
    static int Mnext[]={1,2,3,4,5,6,7,8,9,0},Mind,stationary,stationary2;    
    static BASETYPE1 *w,*qua,*ix;
    static BASETYPE *patchA,*patchB,*ims,*images;
    static BASETYPE2 *patchesA,*patchesB,*res,*imss,*strength,*patches;


    if(qua==NULL)
    {
        
        M=width;
        N=height;
        wx=7;
        wy=7;
        wxy=(2*wx+1)*(2*wy+1);
       
        maxIt=(max_num_of_features+1)*(end-num+step)/step;
        maxV=statedim*window+statedim*2;
        printf("maxIt: %d\n",maxIt); 
        printf("maxV: %d\n",maxV);
        v[7]=param[0];
        v[8]=param[1];
        v[9]=param[2];

        thx=3;
        th=2;

        vz[0]=1.;
        vz[1]=0.;
        vz[2]=0.;
        vz[3]=0.;
        vz[4]=0.;
        vz[5]=0.;
        vz[6]=0.;
        vn[0]=1.;
        vn[1]=0.;
        vn[2]=0.;
        vn[3]=0.;
        vn[4]=0.;
        vn[5]=0.;
        vn[6]=0.;
        W[0]=1.;
        W[1]=1.;
        W[2]=1.;
        W[3]=1.;
        W[4]=1.;
        W[5]=1.;
        W[6]=1.;
        W[7]=1.;
        W[8]=1.;
        W[9]=1.;

        if (( Xpos=(double *)malloc((maxIt*10)*sizeof(double))) == NULL) printf("malloc error");
        if (( XX=(double *)malloc(maxIt*3*sizeof(double))) == NULL) printf("malloc error");
        if (( ptsX=(double *)malloc(maxIt*2*sizeof(double))) == NULL) printf("malloc error");
        if (( maskX=(int *)malloc(maxIt*sizeof(int))) == NULL) printf("malloc error");
        if (( xx=(double *)malloc(maxX*3*sizeof(double))) == NULL) printf("malloc error");
        if (( vx=(double *)malloc(maxV*maxX*sizeof(double))) == NULL) printf("malloc error");
        if (( vv=(double *)malloc(maxV*maxV*sizeof(double))) == NULL) printf("malloc error");
        if (( icb=(double *)malloc(maxV*maxX*sizeof(double))) == NULL) printf("malloc error");
        if (( R=(double *)malloc(maxV*maxV*sizeof(double))) == NULL) printf("malloc error");
        if (( Ce=(double *)malloc(maxD*sizeof(double))) == NULL) printf("malloc error");
        if (( x=(double *)malloc(maxD*sizeof(double))) == NULL) printf("malloc error");
        if (( g=(double *)malloc(maxD*sizeof(double))) == NULL) printf("malloc error");
        if (( L=(double *)malloc(3*maxX*sizeof(double))) == NULL) printf("malloc error");
        if (( fea=(double *)malloc(14*(maxIt)*sizeof(double))) == NULL) printf("malloc error");
        if (( e=(double *)malloc((maxI)*sizeof(double))) == NULL) printf("malloc error");
        if (( z=(double *)malloc(2*(maxI)*sizeof(double))) == NULL) printf("malloc error");
        if (( ind=(int *)malloc(2*(maxI)*sizeof(int))) == NULL) printf("malloc error");
        if (( indf=(int *)malloc(2*(maxI)*sizeof(int))) == NULL) printf("malloc error");
        if (( indz=(int *)malloc(2*(maxI)*sizeof(int))) == NULL) printf("malloc error");
        if (( depth=(double *)malloc((maxI)*sizeof(double))) == NULL) printf("malloc error");
        if (( patches=(BASETYPE2 *)malloc(wxy*(maxIt)*sizeof(BASETYPE2))) == NULL) printf("malloc error");
        if (( res=(BASETYPE2 *)malloc(max_num_of_features*max_num_of_features*sizeof(BASETYPE2))) == NULL) printf("malloc error");
        if (( resi=(double *)malloc(max_num_of_features*max_num_of_features*sizeof(double))) == NULL) printf("malloc error");
        if (( nX=(double *)malloc(max_num_of_features*10*sizeof(double))) == NULL) printf("malloc error");
        if (( indC=(int*)malloc((max_num_of_features)*sizeof(int))) == NULL) printf("malloc error");
        if (( indx=(int*)malloc((maxIt)*sizeof(int))) == NULL) printf("malloc error");
        if (( indX=(int*)malloc((maxIt)*sizeof(int))) == NULL) printf("malloc error");
        if (( mask=(int*)malloc((max_num_of_features)*sizeof(int))) == NULL) printf("malloc error");
        if (( XA=(double *)malloc(maxI*3*sizeof(double))) == NULL) printf("malloc error");
        if (( pA=(double *)malloc(maxI*2*sizeof(double))) == NULL) printf("malloc error");
        if (( X=(double *)malloc(maxD*3*sizeof(double))) == NULL) printf("malloc error");
        if (( matchl=(int *)malloc(maxIt*sizeof(int))) == NULL) printf("malloc error");
        if (( resl=(double *)malloc(maxIt*2*sizeof(double))) == NULL) printf("malloc error");
        if (( matcho=(int *)malloc(max_num_of_features*sizeof(int))) == NULL) printf("malloc error");
        if (( reso=(double *)malloc(2*max_num_of_features*sizeof(double))) == NULL) printf("malloc error");
        if (( qua=(BASETYPE1  *)malloc(N*M*sizeof(BASETYPE1 ))) == NULL) printf("malloc error");
        if (( swap=(int *)malloc(N*M*2*sizeof(int))) == NULL) printf("malloc error");
        if (( ix=(BASETYPE1*)malloc(N*M*sizeof(BASETYPE1))) == NULL) printf("malloc error");
        if (( sindex=(int *)malloc(N*M*sizeof(int))) == NULL) printf("malloc error");
        if (( start=(int *)malloc(( (end-num+step)/step+1)*sizeof(int))) == NULL) printf("malloc error");

        if (( indA=(int *)malloc(max_num_of_features*sizeof(int))) == NULL) printf("malloc error");
        if (( indB=(int *)malloc(max_num_of_features*sizeof(int))) == NULL) printf("malloc error");
        if (( ptA=(double *)malloc(2*max_num_of_features*sizeof(double))) == NULL) printf("malloc error");
        if (( ptB=(double *)malloc(2*max_num_of_features*sizeof(double))) == NULL) printf("malloc error");
        if (( featuresA=(double *)malloc(3*max_num_of_features*sizeof(double))) == NULL) printf("malloc error");
        if (( featuresB=(double *)malloc(3*max_num_of_features*sizeof(double))) == NULL) printf("malloc error");
        if (( fA=(double *)malloc(2*max_num_of_features*sizeof(double))) == NULL) printf("malloc error");
        if (( fB=(double *)malloc(2*max_num_of_features*sizeof(double))) == NULL) printf("malloc error");
        if (( matches=(int *)malloc(max_num_of_features*sizeof(int))) == NULL) printf("malloc error");
        if (( strength=(BASETYPE2 *)malloc(2*max_num_of_features*sizeof(BASETYPE2))) == NULL) printf("malloc error");
        if (( tmp=(int *)malloc(max_num_of_features*sizeof(int))) == NULL) printf("malloc error");
        if (( patchA=(BASETYPE *)malloc(wxy*max_num_of_features*sizeof(BASETYPE))) == NULL) printf("malloc error");
        if (( patchB=(BASETYPE *)malloc(wxy*max_num_of_features*sizeof(BASETYPE))) == NULL) printf("malloc error");
        if (( patchesA=(BASETYPE2 *)malloc(wxy*max_num_of_features*sizeof(BASETYPE2))) == NULL) printf("malloc error");
        if (( patchesB=(BASETYPE2 *)malloc(wxy*max_num_of_features*sizeof(BASETYPE2))) == NULL) printf("malloc error");        
        if (( w=(BASETYPE1 *)malloc(N*M*sizeof(BASETYPE1))) == NULL) printf("malloc error");
        if (( images=(unsigned char *)malloc(width*height*10*sizeof(unsigned char)))==NULL) printf("malloc error");
        
        numpts=0;
        detect_corners(image1,qua,w,M,N);

        sp_find_loc_max_4tile(qua,featuresA,1.,swap,ix,sindex,&num_featuresA,&k,M,N,max_num_of_features);
        sp_extract_patchesn(patchesA,image1,featuresA,num_featuresA, wx, wy, M);
        
        frame=0;
        init_fea(fea,featuresA,&frame,start,num_featuresA);
        for(i=0;i<num_featuresA*wxy;i++)
            patches[i]=patchesA[i];
        memcpy(images,image1,M*N);
        Mind=0;
        frame0=0;
        num_landmarks=1;
        lost=0;

    }
    else
    {
        /* extract new feature points from the image */
        detect_corners(image1,qua,w,M,N);
        sp_find_loc_max_4tile(qua,featuresB,2.,swap,ix,sindex,&num_featuresB,&k,M,N,max_num_of_features);
        if(num_featuresB<7)
          return 0;

        sp_extract_patchesn(patchesB,image1,featuresB,num_featuresB, wx, wy, M);
     
        if(initialized) 
        {   
/*  track features and find next pose  */
            for(i=0;i<max_num_of_features;i++)
                matches[i]=0;

            match_cornerscl(res,strength,matches,patchesA,patchesB,tmp,featuresA,featuresB,num_featuresA,num_featuresB,wxy,M/2,0.5,0.99);
            j=0;
            stationary=0;
            stationary2=0;
            for(i=0;i<max_num_of_features;i++)
            {
                if(matches[i]>0)
                {
                    if(fabs((featuresA[3*i]-featuresB[3*(matches[i]-1)]))+fabs((featuresA[3*i+1]-featuresB[3*(matches[i]-1)+1]))<10)
                        stationary++;
                    if(fabs((featuresA[3*i]-featuresB[3*(matches[i]-1)]))+fabs((featuresA[3*i+1]-featuresB[3*(matches[i]-1)+1]))<1)
                        stationary2++;
                    j++;
                }
            }

            if((stationary*10>9*j)||(stationary2*10>4*j))
            {
                if((num+2*step)>end)
                {       
                    for(i=0;i<frame;i++)
                    {
                        printf("Xpos1:");
                        for(j=0;j<statedim;j++)
                        {
                            printf("%f ",Xpos[i*statedim+j]);
                        }
                        printf("\n");
                    }
                    for(i=0;i<start[frame+1];i++)
                    {
                        printf("fea:");
                        for(j=0;j<fea_dim;j++)
                        {
                            printf("%f ",fea[i*fea_dim+j]);
                        }
                        printf("\n");
                    }
                }
                return 0;
            }
            frame++;
        
            j=0;
            for(i=0;i<max_num_of_features;i++)
                if(matches[i]>0)
                {
                    j++;
                }


            update_fea(fea,featuresB,frame+1,start,num_featuresA,num_featuresB,matches,strength);
        
            for(i=0;i<num_featuresB*wxy;i++)
                patches[start[frame]*wxy+i]=patchesB[i];
            
            parse_matches(fea,indA,indB,mask,start[frame-1],start[frame],&num_pts);
  
            for(i=0;i<num_pts;i++)
            {
                ptA[2*i]=fea[indA[i]*fea_dim];
                ptA[2*i+1]=fea[indA[i]*fea_dim+1];
                ptB[2*i]=fea[indB[i]*fea_dim];
                ptB[2*i+1]=fea[indB[i]*fea_dim+1];
                ptsA[2*i]=ptA[2*i];
                ptsA[2*i+1]=ptA[2*i+1];
                ptsB[2*i]=ptB[2*i];
                ptsB[2*i+1]=ptB[2*i+1];
            }
           

            for(i=0;i<num_landmarks;i++)
                maskX[i]=0;
            k=0;
            cnt=0;
            for(i=0;i<num_pts;i++)
            {
                if(mask[i]==0)
                {
                    cnt++;
                }
                else
                {
                    index=mask[i];
                    index--;
                    pA[2*k]=ptB[2*i];
                    pA[2*k+1]=ptB[2*i+1];
                    XA[3*k]=XX[3*index];
                    XA[3*k+1]=XX[3*index+1];
                    XA[3*k+2]=XX[3*index+2];
                    maskX[index]=1;
                    k++;
                }
            }
            numpts=num_pts;

            k=0;
            if(num_pts>4)
            {
                init_next_pose(v,Xpos+(frame-1)*statedim, param,resi,num_pts-cnt,th,pA,XA,ptA,ptB,num_pts);
                proj3D_err(resi,v,ptB,XX,mask,th,&k,&j,num_pts);
                jsort(resi,j);
                count[2]=k;
            }
/*  refine next pose   */
            if(k>4)
            {
                lambda=0.1;
                if(((frame-frame0)-window)<1)
                {
                    rwindow=(frame-frame0);
                    scyc=2*iter;
                }
                else
                {
                    rwindow=window;
                    scyc=iter;
                }
                parse_args_for_slam(fea,XX,z,ind,indf,Xpos+statedim*(frame+1),indx,indX,indz,&j,&kk,start[frame+1],rwindow);
                D=rwindow*statedim+kk*3;
                for(i=0;i<D;i++)
                    X[i]=Xpos[i+(frame+1-rwindow)*statedim];
                num_frames=rwindow+1;
        
                slam3D_batch1(L,R,x,icb,g,param,&Xout,Xpos,X,ind,indf,z,vv,vx,xx,Ce,C,D,j,num_frames,frame+1-rwindow,lambda,e,.9,scyc);
        
                sume=0;
                for(i=0;i<j;i++)
                    sume=sume+e[i];
        
                hpsrt(e,indx,j);

                for(i=0;i<D;i++)
                    Xpos[i+(frame+1-rwindow)*statedim]=Xout[i];
        
                update_XX(XX,Xpos+statedim*(frame+1),indX,kk);
                Rt_2_t(v,Xpos+frame*statedim);
         
                slam3D_batch_er1(param,Xpos,Xout,ind,indf,z,D,j,num_frames,frame+1-rwindow,&sume,e,depth);
                cntr=0;
                for(i=0;i<j;i++)
                {
                    if(e[i]>3*th)
                        cntr++;
                }
          
                prune_slamfea(fea,indz,e,3.*th,j);
/*  calc rematches  */
                j=0;
                cntr=0;
                for(i=start[frame];i>start[frame-rwindow];i--)
                {
                    index=fea[i*fea_dim+6];
                    if(index>-1&&fea[i*fea_dim+2]==-1)
                    {
                        index=index-1;                
                        find_nearest_point_to_landmark(v,param,fea+start[frame]*fea_dim, XX+index*3,resi,ind,num_featuresB, M,N);
                        if(resi[0]<1.5)
                        {
                            fea[2+i*fea_dim]=start[frame]+ind[0];      /* next*/
                            fea[3+(start[frame]+ind[0])*fea_dim]=i;      /* prev */
                            fea[6+(start[frame]+ind[0])*fea_dim]=fea[6+i*fea_dim];  /* map */
                            j++;
                        }
                        if(resi[0]<900)
                            cntr++;               
                    }
                }

                for(i=0;i<7;i++)
                    v[i]=Xpos[i+frame*statedim];
/* init new landmarks and refine estimates */
                init_structure(v,nX,count,Xpos+(frame-1)*statedim,ptA, ptB,indC,indB,mask,num_pts,thx);

                k=count[3];  
                prune_fea(fea,indB,mask,num_pts);
                
                for(i=0;i<num_pts;i++)
                    maski[i]=mask[i];
            
                inc_XX(XX,fea,nX,indC,&num_landmarks,k);
                lambda=0.1;
                if(((frame-frame0)-window)<1)
                {
                    rwindow=(frame-frame0);
                    scyc=2*iter;
                }
                else
                {
                    rwindow=window;
                    scyc=iter;
                }
                parse_args_for_slam(fea,XX,z,ind,indf,Xpos+statedim*(frame+1),indx,indX,indz,&j,&kk,start[frame+1],rwindow);
                D=rwindow*statedim+kk*3;
                printf("D: %d\n",D);
                for(i=0;i<D;i++)
                    X[i]=Xpos[i+(frame+1-rwindow)*statedim];
            
                num_frames=rwindow+1;
 
                slam3D_batch1(L,R,x,icb,g,param,&Xout,Xpos,X,ind,indf,z,vv,vx,xx,Ce,C,D,j,num_frames,frame+1-rwindow,lambda,e,.9,scyc);
        
                sume=0;
                for(i=0;i<j;i++)
                    sume=sume+e[i];
                hpsrt(e,indx,j);

                for(i=0;i<D;i++)
                    Xpos[i+(frame+1-rwindow)*statedim]=Xout[i];
                update_XX(XX,Xpos+statedim*(frame+1),indX,kk);
       
                Rt_2_t(vz,Xpos+frame*statedim);
		k=0;
                cnt=0;
               for(i=0;i<num_pts;i++)
               {
                   if(mask[i]==0)
                   {
                       cnt++;
                   }
                   else
                   {
                       index=mask[i];
                       index--;
                       pA[2*k]=fea[indA[i]*fea_dim];
                       pA[2*k+1]=fea[indA[i]*fea_dim+1];
                       XA[3*k]=XX[3*index];
                       XA[3*k+1]=XX[3*index+1];
                       XA[3*k+2]=XX[3*index+2];
                       k++;
                   }
               }
	       Rt_2_t(v,Xpos+(frame-1)*statedim);
               while(num_tracked_points<num_tpts)
               {
                  estimate_depth_from_neighbours(tpts+num_tracked_points*2,tX+num_tracked_points*3,v,pA,XA,param,k);
                  num_tracked_points++;
               } 
            }
            else
            {
                k=0;
            /*erase stray tracks */
                for(i=start[frame+1];i>start[frame];i--)
                {
                    fea[fea_dim*i+3]=-1;
                    fea[fea_dim*i+6]=-1;
                    k++;
                }
                X0[0]=Xpos[(frame-1)*statedim];
                X0[1]=Xpos[(frame-1)*statedim+1];
                X0[2]=Xpos[(frame-1)*statedim+2];
                X0[3]=Xpos[(frame-1)*statedim+3];
                X0[4]=Xpos[(frame-1)*statedim+4];
                X0[5]=Xpos[(frame-1)*statedim+5];
                X0[6]=Xpos[(frame-1)*statedim+6];
                frame0=frame;
                numpts=0;
                initialized=0;
                lost=1;
                frame++;
            }

            fs=featuresA;
            featuresA=featuresB;
            featuresB=fs;
            num_featuresA=num_featuresB;
            ims=patchA;
            patchA=patchB;
            patchB=ims;
            imss=patchesA;
            patchesA=patchesB;
            patchesB=imss;
            tnorm=sqrt(Xpos[11]*Xpos[11]+Xpos[12]*Xpos[12]+Xpos[13]*Xpos[13]);

            if((num+2*step)>end)
            {       

                for(i=0;i<=frame;i++)
                {
                    printf("Xpos1:");
                    for(j=0;j<statedim;j++)
                    {
                        printf("%f ",Xpos[i*statedim+j]);
                    }
                    printf("\n");
                }
                printf("heady: %d %d\n",frame,start[frame+1]);
                for(i=0;i<start[frame+1];i++)
                {
                    printf("fea:");
                    for(j=0;j<fea_dim;j++)
                    {
                        printf("%f ",fea[i*fea_dim+j]);
                    }
                    printf("\n");
                }
            }
        }
        else /* we haven't yet initialized because we are starting or we haven' t recovered from tracking failure */
        {
            Xpos[frame0*statedim]=X0[0];
            Xpos[1+frame0*statedim]=X0[1];
            Xpos[2+frame0*statedim]=X0[2];
            Xpos[3+frame0*statedim]=X0[3];
            Xpos[4+frame0*statedim]=X0[4];
            Xpos[5+frame0*statedim]=X0[5];
            Xpos[6+frame0*statedim]=X0[6];  
            for(i=0;i<max_num_of_features;i++)
                matches[i]=0;

            match_cornerscl(res,strength,matches,patchesA,patchesB,tmp,featuresA,featuresB,num_featuresA,num_featuresB,wxy,M/2,0.6,0.99);
  
            j=0;
            stationary=0;
            stationary2=0;
            for(i=0;i<max_num_of_features;i++)
            {
                if(matches[i]>0)
                {
                    if(fabs((featuresA[3*i]-featuresB[3*(matches[i]-1)]))+fabs((featuresA[3*i+1]-featuresB[3*(matches[i]-1)+1]))<10)
                        stationary++;
                    if(fabs((featuresA[3*i]-featuresB[3*(matches[i]-1)]))+fabs((featuresA[3*i+1]-featuresB[3*(matches[i]-1)+1]))<1)
                        stationary2++;
                    j++;
                }
            }
            if(stationary*10>9*j)
                return 0;
            if(stationary2*10>4*j)
                return 0;
            if(j<40)
            {
                Mind=Mnext[Mind];
                memcpy(images+Mind*M*N,image1,M*N);
                for(i=0;i<max_num_of_features;i++)
                    matches[i]=0;
                update_fea(fea,featuresB,frame+1,start,num_featuresA,num_featuresB,matches,strength );
                for(i=0;i<num_featuresB*wxy;i++)
                patches[start[frame]*wxy+i]=patchesB[i];
                X0[0]=Xpos[(frame-1)*statedim];
                X0[1]=Xpos[(frame-1)*statedim+1];
                X0[2]=Xpos[(frame-1)*statedim+2];
                X0[3]=Xpos[(frame-1)*statedim+3];
                X0[4]=Xpos[(frame-1)*statedim+4];
                X0[5]=Xpos[(frame-1)*statedim+5];
                X0[6]=Xpos[(frame-1)*statedim+6];
                frame0=frame;
                numpts=0;
                frame++;
                fs=featuresA;
                featuresA=featuresB;
                featuresB=fs;
                num_featuresA=num_featuresB;
                ims=patchA;
                patchA=patchB;
                patchB=ims;
                imss=patchesA;
                patchesA=patchesB;
                patchesB=imss;
                return 2;
            }

            Mind=Mnext[Mind];
            memcpy(images+Mind*M*N,image1,M*N);
            update_fea(fea,featuresB,frame+1,start,num_featuresA,num_featuresB,matches,strength);
            for(i=0;i<num_featuresB*wxy;i++)
                patches[start[frame]*wxy+i]=patchesB[i];
            parse_matches(fea,indA,indB,mask,start[frame-1],start[frame],&num_pts); 



            for(i=0;i<num_pts;i++)
            {
                ptA[2*i]=fea[indA[i]*fea_dim];
                ptA[2*i+1]=fea[indA[i]*fea_dim+1];
                ptB[2*i]=fea[indB[i]*fea_dim];
                ptB[2*i+1]=fea[indB[i]*fea_dim+1];
                ptsA[2*i]=ptA[2*i];
                ptsA[2*i+1]=ptA[2*i+1];
                ptsB[2*i]=ptB[2*i];
                ptsB[2*i+1]=ptB[2*i+1];
            }

            numpts=num_pts;
            normalise_points(fA,ptA,param,num_pts);
            normalise_points(fB,ptB,param,num_pts);
            cyc=0;
            do
            {
                ransac_calc_esse7(v,v+4,fA,fB, resi,dev,1./param[0],num_pts);
                cyc++;
                if(cyc>100)
                    break;
            }
            while(sqrt(dev[0])*param[0]>1);
        
            t_2_Rt(vn,v);
            motion3D_update1(Xpos+frame0*statedim,vn,v);
            for(i=0;i<statedim;i++)
                Xpos[i+frame*statedim]=v[i];

            robust_triangulate1(nX,X0,v,ptA,ptB,indB,indC,mask,thx,&k,num_pts);
            count[3]=k;

            num_frames=frame+1;
            prune_fea(fea,indB,mask,num_pts);
            for(i=0;i<num_pts;i++)
                maski[i]=mask[i];
    
            inc_XX(XX,fea,nX,indC,&num_landmarks,k);
            lambda=0.1;
            if(((frame-frame0)-window)<1)
            {
                rwindow=(frame-frame0);
                scyc=2*iter;
            }
            else
            {
                rwindow=window;
                scyc=iter;
            }
            parse_args_for_slam(fea,XX,z,ind,indf,Xpos+statedim*(frame+1),indx,indX,indz,&j,&kk,start[frame+1],rwindow);//fixme pose
            printf("stati: %d %d %d %d\n",frame,kk,stationary2,stationary);
            if(kk-stationary2>8)
            {
                lambda=0.1;
                D=rwindow*statedim+kk*3;
                printf("D: %d\n",D);
                for(i=0;i<D;i++)
                     X[i]=Xpos[i+(frame+1-rwindow)*statedim];
                num_frames=rwindow+1;
        
                slam3D_batch1(L,R,x,icb,g,param,&Xout,Xpos,X,ind,indf,z,vv,vx,xx,Ce,C,D,j,num_frames,frame+1-rwindow,lambda,e,.9,scyc);

                sume=0;
                for(i=0;i<j;i++)
                    sume=sume+e[i];
                hpsrt(e,sindex,j);

        
                for(i=0;i<D;i++)
                    Xpos[i+frame*statedim]=Xout[i];
                update_XX(XX,Xpos+statedim*(frame+1),indX,kk); 
                for(i=0;i<7;i++)
                    vz[i]=Xpos[i+frame*statedim];
        
        
                printf("initialised\n");
                initialized=1;
                lost=0;
                fs=featuresA;
                featuresA=featuresB;
                featuresB=fs;
                num_featuresA=num_featuresB;
                ims=patchA;
                patchA=patchB;
                patchB=ims;
                imss=patchesA;
                patchesA=patchesB;
                patchesB=imss;
                tnorm=sqrt(Xpos[11]*Xpos[11]+Xpos[12]*Xpos[12]+Xpos[13]*Xpos[13]);

            }
            else
            {
                k=0;
                /*erase stray tracks */
                for(i=start[frame+1];i>start[frame];i--)
                {
                    fea[fea_dim*i+3]=-1;
                    fea[fea_dim*i+6]=-1;
                    k++;
                }
                printf("erased: %d %d %d\n",frame,start[frame],k);
                X0[0]=Xpos[(frame-1)*statedim];
                X0[1]=Xpos[(frame-1)*statedim+1];
                X0[2]=Xpos[(frame-1)*statedim+2];
                X0[3]=Xpos[(frame-1)*statedim+3];
                X0[4]=Xpos[(frame-1)*statedim+4];
                X0[5]=Xpos[(frame-1)*statedim+5];
                X0[6]=Xpos[(frame-1)*statedim+6];
                frame0=frame;
                numpts=0;
                frame++;
                fs=featuresA;
                featuresA=featuresB;
                featuresB=fs;
                num_featuresA=num_featuresB;
                ims=patchA;
                patchA=patchB;
                patchB=ims;
                imss=patchesA;
                patchesA=patchesB;
                patchesB=imss;
                return 2;
            }
        }
    }
    return 1;
}


